/*
Continue from RFC4944 section 6
*/

#include <string.h>

/* TYPES */
typedef unsigned char uint8;
typedef short unsigned int uint16;
typedef unsigned int uint32;
typedef long unsigned int uint64;

typedef uint8 Bool;
typedef uint8 Enum;
typedef uint8 Octet;
typedef uint64 Address;



/* CONSTANTS */
/* RFC 4944 Section 4 */
#define MAX_PACKET_SIZE 127
/* RFC4944 Section 5.3 */
#define MTU 1280
/* RFC4944 Section 5.3; in seconds */
#define MAX_REASSEMBLY_TIMEOUT 60
/* RFC 4944 Section 3 */
Address BROADCAST = 0x0000FFFF;
/* RFC4944 Section 5 */
uint8 DISPATCH_MASK = 0xC0;
uint8 HEADER_IPV6 = 0x01;
uint8 HEADER_MESH = 0x02;
uint8 HEADER_BROADCAST = 0x04;
uint8 HEADER_FRAG = 0x08;
uint8 HEADER_HC1 = 0x10;
/* RFC4944 Section 5.1, Figure 2 */
Octet DISPATCH_NALP = 0xC0;
Octet DISPATCH_IPV6 = 0x41;
Octet DISPATCH_LOWPAN_HC1 = 0x42;
Octet DISPATCH_LOWPAN_BCO = 0x50;
Octet DISPATCH_ESC = 0x7F;
Octet DISPATCH_MESH = 0x80;
Octet DISPATCH_FRAG_MASK = 0xF8;
Octet DISPATCH_FRAG1 = 0xC0;
Octet DISPATCH_FRAGN = 0xE0;
/* RFC4944 Section 5.2 */
Octet MESH_ORIG_ADDR_MASK = 0x20;
Octet MESH_FINAL_ADDR_MASK = 0x10;
Octet MESH_HOPS_MASK = 0x0F;
Octet MESH_HOPS_EXTENDED = 0x0F;
uint8 SHORT_ADDRESS_SIZE = 2;
uint8 LONG_ADDRESS_SIZE = 8;
/* RFC4944 Section 5.3 */
uint8 FRAG1_HEADER_SIZE = 4;
uint8 FRAGN_HEADER_SIZE = 5;
Octet FRAG_DATAGRAM_SIZE_MASK = 0x07;



/* STRUCTS */
typedef struct {
	int tmp;
} PacketFactory6LoWPAN;

typedef struct {
	Octet pktBuf[MAX_PACKET_SIZE];
} Packet6LoWPAN;



/* FUNCTION FORWARD DECLARATIONS */
Bool is6LoWPANFrame(Octet);
Bool isIPv6Header(Octet);
Bool isHC1Header(Octet);
Bool isMeshHeader(Octet);
Bool isFrag1Header(Octet);
Bool isFragNHeader(Octet);
Bool isEscapeHeader(Octet);



/* GLOBALS */



int main(int argc, char** argv) {
	return 0;
}



/* FUNCTION DEFINITIONS */

/*TODO packets should be dropped if this returns false */
/* RFC4944 Section 5.1 */
Bool is6LoWPANFrame(Octet dispatch) {
	return dispatch & DISPATCH_MASK;
}

/* RFC4944 Section 5.1 */
Bool isIPv6Header(Octet dispatch) {
	return dispatch == HEADER_IPV6;
}

/* RFC4944 Section 5.1 */
Bool isHC1Header(Octet dispatch) {
	return dispatch == HEADER_HC1;
}

/* RFC4944 Section 5.1 */
Bool isMeshHeader(Octet dispatch) {
	return (dispatch & DISPATCH_MASK) == DISPATCH_MESH;
}

/* RFC4944 Section 5.1 */
Bool isFrag1Header(Octet dispatch) {
	return (dispatch & DISPATCH_FRAG_MASK) == DISPATCH_FRAG1;
}

/* RFC4944 Section 5.1 */
Bool isFragNHeader(Octet dispatch) {
	return (dispatch & DISPATCH_FRAG_MASK) == DISPATCH_FRAGN;
}

/* RFC4944 Section 5.1 */
Bool isEscapeHeader(Octet dispatch) {
	return dispatch == DISPATCH_ESC;
}

/* RFC4944 Section 5.2 */
/* If isExtendedHops is FALSE, the first 5 bits of hops should all be 0 */
/* Returns the size of the header */
uint32 packMeshHeader(Octet* start, Bool isLongOrig, Address orig, Bool isLongDest, Address dest, Bool isExtendedHops, uint32 hops) {
	uint8 headerSize;

	headerSize = 1;
	start[0] = DISPATCH_MESH;

	if (isExtendedHops) {
		start[0] = start[0] | MESH_HOPS_EXTENDED;
		start[1] = hops;
		++headerSize;
	} else {
		start[0] = start[0] | hops;
	}

	if (isLongOrig) {
		memcpy(start + headerSize, &orig, LONG_ADDRESS_SIZE);
		headerSize += LONG_ADDRESS_SIZE;
	} else {
		start[0] = start[0] | MESH_ORIG_ADDR_MASK;
		memcpy(start + headerSize, &orig, SHORT_ADDRESS_SIZE);
		headerSize += SHORT_ADDRESS_SIZE;
	}

	if (isLongDest) {
		memcpy(start + headerSize, &dest, LONG_ADDRESS_SIZE);
		headerSize += LONG_ADDRESS_SIZE;
	} else {
		start[0] = start[0] | MESH_FINAL_ADDR_MASK;
		memcpy(start + headerSize, &dest, SHORT_ADDRESS_SIZE);
		headerSize += SHORT_ADDRESS_SIZE;
	}

	return headerSize;
}

/* RFC4944 Section 5.2 */
//TODO may need to return whether the address is long or short
/* Returns the size of the header */
uint8 unpackMeshHeader(Octet* start, Address* orig, Address* dest, uint32* hops, Octet** end) {
	unsigned int hopsLeft;
	Octet isShortOrig, isShortDest;
	uint8 headerSize;

	headerSize = 1;
	isShortOrig = start[0] & MESH_ORIG_ADDR_MASK;
	isShortDest = start[0] & MESH_FINAL_ADDR_MASK;

	hopsLeft = MESH_HOPS_MASK & start[0];
	if (hopsLeft == MESH_HOPS_EXTENDED) {
		(*hops) = start[1];
		++headerSize;
	} else {
		(*hops) = hopsLeft;
	}

//TODO I believe there is some additional processing to convert a 16 bit address to a 64 bit one. I need to confirm this.
	if (isShortOrig) {
// any disadvantages to using memcpy?
		memcpy(orig, start + headerSize, SHORT_ADDRESS_SIZE);
		headerSize += SHORT_ADDRESS_SIZE;
	} else {
		memcpy(orig, start + headerSize, LONG_ADDRESS_SIZE);
		headerSize += LONG_ADDRESS_SIZE;
	}

	if (isShortDest) {
		memcpy(orig, start + headerSize, 2);
		headerSize += SHORT_ADDRESS_SIZE;
	} else {
		memcpy(orig, start + headerSize, LONG_ADDRESS_SIZE);
		headerSize += LONG_ADDRESS_SIZE;
	}

	return headerSize;
}

/* RFC4944 Section 5.3 */
/* The first 5 bits of datagram size should all be 0 */
void packFrag1Header(Octet* start, uint16 datagramSize, uint16 datagramTag) {
	start[0] = DISPATCH_FRAG1;
	start[0] |= (Octet) (datagramSize >> 8);
	start[1] = (Octet) (datagramSize);
	start[2] = (Octet) (datagramTag >> 8);
	start[3] = (Octet) (datagramTag);
}

/* RFC4944 Section 5.3 */
/* The first 5 bits of datagram size should all be 0 */
void packFragNHeader(Octet* start, uint16 datagramSize, uint16 datagramTag, uint8 datagramOffset) {
	packFrag1Header(start, datagramSize, datagramTag);
	start[4] = (Octet) datagramOffset;
}

/* RFC4944 Section 5.3 */
/* start includes the header identifier */
void unpackFrag1Header(Octet* start, uint16* datagramSize, uint16* datagramTag) {
	(*datagramSize) = ((start[0] & FRAG_DATAGRAM_SIZE_MASK) << 8) | start[1];
	(*datagramTag) = (start[2] << 8) | start[3];
}

/* RFC4944 Section 5.3 */
/* start includes the header identifier */
void unpackFragNHeader(Octet* start, uint16* datagramSize, uint16* datagramTag, uint8* datagramOffset) {
	unpackFrag1Header(start, datagramSize, datagramTag);
	(*datagramOffset) = start[4];
}